#include <iostream>
#include <cmath>
#include "Matrix.h"

namespace MathGraphics
{
	//From PBRT:
	Matrix Matrix::inverse() const 
	{
		int indxc[4], indxr[4];
		int ipiv[4] = {0};
		double minv[4][4];
		
		memcpy(minv, d, 4*4*sizeof(double));
		
		for (int i = 0; i < 4; i++) 
		{
			int irow = -1, icol = -1;
			double big = 0.;
			// Choose pivot
			for (int j = 0; j < 4; j++) 
			{
				if (ipiv[j] != 1) {
					for (int k = 0; k < 4; k++) 
					{
						if (ipiv[k] == 0) 
						{
							if (fabsf(minv[j][k]) >= big) 
							{
								big = double(fabsf(minv[j][k]));
								irow = j;
								icol = k;
							}
						}
						else if (ipiv[k] > 1)
							std::cerr<<"Singular matrix in MatrixInvert"<<std::endl;
					}
				}
			}
			++ipiv[icol];
			// Swap rows _irow_ and _icol_ for pivot
			if (irow != icol) {
				for (int k = 0; k < 4; ++k)
					std::swap(minv[irow][k], minv[icol][k]);
			}
			indxr[i] = irow;
			indxc[i] = icol;
			if (minv[icol][icol] == 0.)
				std::cerr<<"Singular matrix in MatrixInvert"<<std::endl;
			// Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
			double pivinv = 1.f / minv[icol][icol];
			minv[icol][icol] = 1.f;
			for (int j = 0; j < 4; j++)
				minv[icol][j] *= pivinv;
			// Subtract this row from others to zero out their columns
			for (int j = 0; j < 4; j++) 
			{
				if (j != icol) 
				{
					double save = minv[j][icol];
					minv[j][icol] = 0;
					for (int k = 0; k < 4; k++)
						minv[j][k] -= minv[icol][k]*save;
				}
			}
		}
		// Swap columns to reflect permutation
		for (int j = 3; j >= 0; j--) 
		{
			if (indxr[j] != indxc[j]) 
			{
				for (int k = 0; k < 4; k++)
					std::swap(minv[k][indxr[j]], minv[k][indxc[j]]);
			}
		}
		return Matrix(minv);
	}
}